// eee.Sheffield.PZ.Imaging
//
// Copyright ?Ping Zou, 2007
// sg71.cherub@gmail.com

namespace eee.Sheffield.PZ.Imaging
{
	using System;
	using System.IO;
	using System.Drawing;
	using System.Drawing.Imaging;
    using eee.Sheffield.PZ.Math;
	using System.Collections;
	
	/// <summary>
	/// consequtive scanline tracking
	/// </summary>
	public class LookForwardSearch
	{
		public PZPoint _initcp = null;		// initial center point;
		public PZDirection _initvd = null;	// initial vessel direction;
		public double _meanr = 0.0;		// mean r estimate
		public ArrayList _cpArrayNear = new ArrayList();	// near set
		public ArrayList _cpArrayFar = new ArrayList();	// far set
		public ArrayList _cpCandidates = new ArrayList();	// center point candidates
		public ArrayList _vdCandidates = new ArrayList();	// vessel direction candidates
		public double _theta = 9.0;		// search step;
		public double _range = 180.0;
        //public double _range = 270.0;

		/// <summary>
		/// empty constructor
		/// </summary>
		public LookForwardSearch() {}

		public LookForwardSearch(PZPoint inputcp, PZDirection inputvd, double inputr)
		{
			_initcp = inputcp;
			_initvd = inputvd;
			_meanr = inputr;
		} // LookForwardSearch(PZPoint, PZDirection, double)

		/// <summary>
		/// look forward search 
		/// </summary>
		/// <param name="srcImg"></param>
		/// <returns></returns>
		public Bitmap Apply(Bitmap srcImg)
		{
			// -- prepare varibales;
			int width = srcImg.Width;
			int height = srcImg.Height;

			double c = 8.0;
			double r = _meanr;
			double mu = 0.02;
			double B = 30.0;
			int widthOfScanline = (int) (3 * r);
			if (widthOfScanline < 12)
				widthOfScanline =  12;
			//if (widthofscanline > 16)
			//	widthofscanline = 16;
			int sampleRate = 1;
			int halfWidth = widthOfScanline / 2;
			

			// debug
            string searchRecordFileName = "searchRecord.txt";
            FileStream searchRecordFileStream = new FileStream(searchRecordFileName, FileMode.Create, FileAccess.Write);
            StreamWriter searchRecordStreamWriter = new StreamWriter(searchRecordFileStream);
            searchRecordStreamWriter.WriteLine("Search started at:");
            searchRecordStreamWriter.WriteLine(String.Format("{0, -7} {1, -7} {2, -7} {3, -7}",
                "vd_x", "vd_y", "cp_x", "cp_y"));
            searchRecordStreamWriter.WriteLine(String.Format("{0, -7:f} {1, -7:f} {2, -7:f} {3, -7:f}",
                _initvd.x, _initvd.y, _initcp.x, _initcp.y));

			// do search
			_initvd.Rotate(-1.0 * _range / 2.0 - _theta);
			int n = (int) (_range / _theta);
			double forwardLengthNear = 2 * _meanr;
			double forwardLengthFar = 3 * _meanr;

            searchRecordStreamWriter.WriteLine();
            searchRecordStreamWriter.WriteLine("searching...");
            searchRecordStreamWriter.Write(String.Format("{0, -4}", ""));
            searchRecordStreamWriter.WriteLine(String.Format("{0, -7} {1, -7} {2, -7} {3, -7} {4, -7} {5, -7}",
                "vd.x", "vd.y", "cp_near.x", "cp_near.y", "cp_far.x", "cp_far.y"));

			for (int i = 0; i <= n; i ++)
			{
				// prepare current look forward direction
				_initvd.Rotate(_theta);
				// prepare center point
				PZPoint tempCPNear = new PZPoint();
				tempCPNear.ExtendFrom(_initcp, _initvd, forwardLengthNear);
				PZPoint tempCPFar = new PZPoint();
				tempCPFar.ExtendFrom(_initcp, _initvd, forwardLengthFar);

                searchRecordStreamWriter.Write(String.Format("{0, -4}", (i + ") ")));
                searchRecordStreamWriter.Write(String.Format("{0, -7:0.000} {1, -7:0.00} {2, -7:0.00} {3, -7:0.00} {4, -7:0.00} {5, -7:0.00}",
                    _initvd.x, _initvd.y, tempCPNear.x, tempCPNear.y, tempCPFar.x, tempCPFar.y));
				
				// inner circle
				// prepare extract scanline
				PZDirection sd = new PZDirection(_initvd);
				sd.GetNormalDirection();

				ExtractScanline exsl = new ExtractScanline(tempCPNear, sd, sampleRate, widthOfScanline);
				exsl.Apply(srcImg);

				// prepare PZscanline near
				PZScanline tempSLNear = new PZScanline(widthOfScanline, sampleRate, c, r, mu, B);

				// fitting the scanline
                tempSLNear._fg.MemCopyFrom(exsl.Fg);
                Array.Copy(exsl.Fp, tempSLNear._fp, exsl.Fp.Length);
				tempSLNear.NMMin();

                if (!tempSLNear.IsFit())
                {
                    // shrink scanline and try again
                    ExtractScanline exShrinkSL = new ExtractScanline(tempCPNear, sd, sampleRate, (int)(widthOfScanline * 0.6));
                    exShrinkSL.Apply(srcImg);
                    PZScanline shrinkSL = new PZScanline(exShrinkSL.Fg.Size, sampleRate, c, r, mu, B);
                    shrinkSL._fg.MemCopyFrom(exShrinkSL.Fg);
                    Array.Copy(exShrinkSL.Fp, shrinkSL._fp, exShrinkSL.Fp.Length);
                    shrinkSL.NMMin();
                    if (shrinkSL.IsFit())
                        tempSLNear = shrinkSL;
                }

                Bitmap tempSourceImage = new Bitmap(srcImg);
				tempSLNear.WriteFile("searchScanlineNear.txt");
                tempSLNear.DrawScanline("scanlineImage.bmp", tempSourceImage);

				if (tempSLNear.IsFit())
				{
					tempCPNear.ExtendFrom(tempCPNear, sd, tempSLNear._c - halfWidth);
					_cpArrayNear.Add(tempCPNear);

                    searchRecordStreamWriter.Write(String.Format(" # near # {0, -7:f} {1, -7:f}",
                        tempCPNear.x, tempCPNear.y));
				}

				// outer circle
				exsl = new ExtractScanline(tempCPFar, sd, sampleRate, widthOfScanline);
				exsl.Apply(srcImg);

				// prepare PZscanline far
				PZScanline tempSLFar = new PZScanline(widthOfScanline, sampleRate, c, r, mu, B);

				// fitting the scanline
                tempSLFar._fg.MemCopyFrom(exsl.Fg);
                Array.Copy(exsl.Fp, tempSLFar._fp, exsl.Fp.Length);
                tempSLFar.NMMin();

                if (!tempSLFar.IsFit())
                {
                    // shrink scanline and try again
                    ExtractScanline exShrinkSL = new ExtractScanline(tempCPFar, sd, sampleRate, (int)(widthOfScanline * 0.6));
                    exShrinkSL.Apply(srcImg);
                    PZScanline shrinkSL = new PZScanline(exShrinkSL.Fg.Size, sampleRate, c, r, mu, B);
                    shrinkSL._fg.MemCopyFrom(exShrinkSL.Fg);
                    Array.Copy(exShrinkSL.Fp, shrinkSL._fp, exShrinkSL.Fp.Length);
                    shrinkSL.NMMin();
                    if (shrinkSL.IsFit())
                        tempSLFar = shrinkSL;
                }

                tempSLFar.WriteFile("searchScanlineFar.txt");
                tempSLFar.DrawScanline("scanlineImage.bmp", tempSourceImage);   // overlap near/far scanline together


                if (tempSLFar.IsFit())
				{
                    tempCPFar.ExtendFrom(tempCPFar, sd, tempSLFar._c - halfWidth);
					_cpArrayFar.Add(tempCPFar);

                    searchRecordStreamWriter.Write(String.Format(" # far # {0, -7:0.000} {1, -7:0.00}",
                        tempCPFar.x, tempCPFar.y));
				}

                searchRecordStreamWriter.WriteLine();
			}

            searchRecordStreamWriter.Close();
            searchRecordFileStream.Close();
		
			// find candidates
			FindCPCandidates();
			
			// create new image
            Bitmap dstImg = new Bitmap(width, height, PixelFormat.Format8bppIndexed);
            return dstImg;
		} // Apply()

        /// <summary>
        /// find centre point candidates for inner/outer layer
        /// </summary>
		public void FindCPCandidates()
		{
			ArrayList tempCandidateNear = new ArrayList();
			ArrayList tempCandidateFar = new ArrayList();
			ArrayList tempP = new ArrayList();

			double theta_rad = _theta / 180.0 * System.Math.PI; 
			//double interval = 4 * _meanr * theta_rad;

			// search inner circle, group those are close
			for (int i = 0; i < _cpArrayNear.Count - 1; i ++)
			{
				// search for close center points
				bool add = false;
				bool search = true;
				while (search && i < _cpArrayNear.Count - 1)
				{
					double dist = (_cpArrayNear[i] as PZPoint).Distance((_cpArrayNear[i + 1] as PZPoint));
					if (dist < 2.0)
					{
						tempP.Add(_cpArrayNear[i]);
						add = true;
						i ++;
					}
					else
					{
						search = false;
					}
				}
				if (add)
					tempP.Add(_cpArrayNear[i]);

				// add candidate
                if (tempP.Count > 0)
                    // add average point
                {
                    PZPoint cpCandidate = new PZPoint();
                    for (int j = 0; j < tempP.Count; j++)
                    {
                        cpCandidate.x += (tempP[j] as PZPoint).x;
                        cpCandidate.y += (tempP[j] as PZPoint).y;
                    }
                    cpCandidate.x /= tempP.Count;
                    cpCandidate.y /= tempP.Count;
                    tempCandidateNear.Add(cpCandidate);
                }
                else
                {
                    tempCandidateNear.Add(_cpArrayNear[i]);
                    if (i == _cpArrayNear.Count - 2)
                        tempCandidateNear.Add(_cpArrayNear[i + 1]);
                }

				tempP.Clear();
			}

			// search outer circle, group those close
			for (int i = 0; i < _cpArrayFar.Count - 1; i ++)
			{
				// search for close center points
				bool add = false;
				bool search = true;
				while (search && i < _cpArrayFar.Count - 1)
				{
					double dist = (_cpArrayFar[i] as PZPoint).Distance((_cpArrayFar[i + 1] as PZPoint));
					if (dist < 2.0)
					{
						tempP.Add(_cpArrayFar[i]);
						add = true;
						i ++;
					}
					else
					{
						search = false;
					}
				}
				if (add)
					tempP.Add(_cpArrayFar[i]);

				// add candidate
                if (tempP.Count > 0)
                // add average point
                {
                    PZPoint cpCandidate = new PZPoint();
                    for (int j = 0; j < tempP.Count; j++)
                    {
                        cpCandidate.x += (tempP[j] as PZPoint).x;
                        cpCandidate.y += (tempP[j] as PZPoint).y;
                    }
                    cpCandidate.x /= tempP.Count;
                    cpCandidate.y /= tempP.Count;
                    tempCandidateFar.Add(cpCandidate);
                }
                else
                {
                    tempCandidateFar.Add(_cpArrayFar[i]);
                    if (i == _cpArrayFar.Count - 2)
                        tempCandidateFar.Add(_cpArrayFar[i + 1]);
                }

				tempP.Clear();
			}

            if (tempCandidateNear.Count != 0 && tempCandidateFar.Count != 0)
            {
                // save to cp_candidates and vd_candidates
                //if (tempCandidateNear.Count == tempCandidateFar.Count)
                //{
                //    for (int i = 0; i < tempCandidateNear.Count; i++)
                //    {
                //        PZDirection vdCandidate = new PZDirection();
                //        vdCandidate.GetDirectionFrom2Points(
                //            (tempCandidateNear[i] as PZPoint),
                //            (tempCandidateFar[i] as PZPoint));
                //        _cpCandidates.Add(tempCandidateNear[i] as PZPoint);
                //        _vdCandidates.Add(vdCandidate);
                //    }
                //}
                //else
                //{
                    for (int i = 0; i < tempCandidateNear.Count; i++)
                    {
                        for (int j = 0; j < tempCandidateFar.Count && i < tempCandidateNear.Count; j++)
                        {                            
                            PZPoint pNear = tempCandidateNear[i] as PZPoint;
                            PZPoint pFar = tempCandidateFar[j] as PZPoint;

                            if (pNear.Distance(pFar) < 1.5 * _meanr)
                            {
                                PZDirection vd_candidate = new PZDirection();
                                vd_candidate.GetDirectionFrom2Points(pNear, pFar);
                                _cpCandidates.Add(pNear);
                                _vdCandidates.Add(vd_candidate);
                                //temp_candidate_near.RemoveAt(i);
                            }
                        }
                    }
                //}

                // debug
                string candidatesRecordFileName = "candidatesRecord.txt";
                FileStream candidateRecordFileStream = new FileStream(candidatesRecordFileName, FileMode.Create, FileAccess.Write);
                StreamWriter candidateRecordStreamWriter = new StreamWriter(candidateRecordFileStream);
                for (int i = 0; i < _cpCandidates.Count; i++)
                {
                    PZPoint tempCP = new PZPoint(_cpCandidates[i] as PZPoint);
                    candidateRecordStreamWriter.WriteLine(String.Format("{0, -7:0.000} {1, -7:0.00}",
                        tempCP.x, tempCP.y));
                }
                candidateRecordStreamWriter.Close();
                candidateRecordFileStream.Close();
            }
		} // FindCPCandidates()

	} // LookForwardSearch
}
